#include <Color.h>
#include <ColorConvert.h>
#include <LinearColorMap.h>
#include <LinearInterpolationMap.h>
#include <ValuePoint.h>
#include <LinearColorMap.h>
#include <Modulation.h>
#include <Average.h>
#include "Tlc5940.h"

#define REDPIN 5
#define GREENPIN 3
#define BLUEPIN 6
LinearColorMap* colorMap;
Average* valueAverage;
Average* maxAverage;
Modulation* modulation;
int maxDistance;
int maxAverageDistance;
int modulationDistance;
int maxValue = 1000;

void setup() {
  Serial.begin(9600);
  Tlc.init(4095);
  analogWrite(REDPIN, 30);
  modulation = newModulation();
  modulation->modulationTime = 3000;
  modulation->minimum = 0.0;
  modulation->maximum = .2;
  maxAverage = newAverageWithSampleSize(5000);
  valueAverage = newAverageWithSampleSize(500);
  createLinearColorMap();
  calibrateMaxDistance();
  analogWrite(REDPIN, 0);
  analogWrite(BLUEPIN, 30);
  delay(1000);
}

void calibrateMaxDistance() {
  delay(1000);
  for (int i = 0; i < 5000; i++) {
    maxAverage->addValue(analogRead(0));
    delay(1);
  }
  maxDistance = maxAverage->average;
  modulationDistance = maxDistance * .75;
}

void loop() {
  int distance = analogRead(0);
  distance = valueAverage->averageWithValue(distance);
  distance = constrain(distance, 10, maxDistance);
  /*
  Serial.print("maxDistance: ");
  Serial.print(maxDistance);
  Serial.print(" distance: ");
  Serial.print(distance);
  Serial.println(" ");
  
  delay(100);
  */  
  sendColorForDistance(distance);
}

void sendColorForDistance(int distance) {
  RGB color;    
  int value = maxValue - map(distance, 10, maxDistance, 0, maxValue);
  color = colorMap->getColorForValue(value);    
  if (distance > modulationDistance) {
    color = modulate(color);
  }
  sendColorToPin(REDPIN, color.r);
  sendColorToPin(GREENPIN, color.g);
  sendColorToPin(BLUEPIN, color.b);
}

void sendColorToPin(int pin, int color) {
  int value = constrain(color, 0, 255);
  analogWrite(pin, value);
}

RGB modulate(RGB color) {
  HSV hsvColor = RGBtoHSV(color);
  hsvColor.v = modulation->nextModulationValue();
  color = HSVtoRGB(hsvColor);   
  return color;
}


void createLinearColorMap() {
  colorMap = newLinearColorMap();
  int numberOfPoles = 12;
  int poles[] = { 0, 300, 440, 480, 520, 580, 650, 750, 800, 860, 900, 1000  };
  int red[] = { 30, 80, 255, 255, 255, 0, 0, 0, 0, 255, 255, 180 };
  int green[] = { 0, 0, 0, 0, 255, 255, 255, 0, 0, 0, 0, 180 };
  int blue[] = { 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 180 };
  for (int i = 0; i < numberOfPoles; i++) {
    colorMap->addRedPoint(poles[i], red[i]);
    colorMap->addGreenPoint(poles[i], green[i]);
    colorMap->addBluePoint(poles[i], blue[i]);
  }
}
